import time
import numpy as np
import cv2

def extract_yellow_region(image):
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    
    lower_yellow = np.array([20, 100, 180])
    upper_yellow = np.array([30, 150, 255])

    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

    yellow_region = cv2.bitwise_and(image, image, mask=mask)

    gray = cv2.cvtColor(yellow_region, cv2.COLOR_BGR2GRAY)

    _, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY)

    return binary
    
    
def getEdgeImg(binary_img):
    canny_lth = 50
    canny_hth = 100
    edgeImg = cv2.Canny(binary_img, canny_lth, canny_hth)
    return edgeImg
    
    
def roi_mask(img, corner_points):
    mask = np.zeros_like(img)
    cv2.fillPoly(mask, corner_points, 255)
    masked_img = cv2.bitwise_and(img, mask)
    return masked_img
    
    
def process_image(image):
    rows, cols = image.shape

    for i in range(rows):
        for j in range(cols - 1, -1, -1):

            if image[i][j] != 0:

                for k in range(j, max(-1, j - 3), -1):
                    if k >= 0:
                        image[i][k] = 0
                break

    return image
    
    
def getRLaneLine(roi_edges):
    rho = 1
    theta = np.pi / 180
    threshold = 10
    min_line_len = 20
    max_line_gap = 5

    lines = cv2.HoughLinesP(roi_edges, rho, theta, threshold, minLineLength=min_line_len, maxLineGap=max_line_gap)

    if lines is not None:
        filtered_lines = []
        for line in lines:
            x1, y1, x2, y2 = line[0]
            slope = (y2 - y1) / (x2 - x1)
            if slope > 0:  # filer the right lines
                filtered_lines.append(line)

        if filtered_lines:

            longest_line = max(filtered_lines, key=lambda x: np.linalg.norm(x[0][0:2] - x[0][2:4]))
            x1, y1, x2, y2 = longest_line[0]

            slope = (y2 - y1) / (x2 - x1)
            b = y2 - slope * x2
            return b,slope
        else:
            return -1.1
    else:
        return -1
        
        
def get_x0(img_path):
    imgOri = cv2.imread(img_path)
    small_img = cv2.resize(imgOri, (int(imgOri.shape[1] / 4), int(imgOri.shape[0] / 4)))
    roi_binary = extract_yellow_region(small_img)
    edges = getEdgeImg(roi_binary)

    rows, cols = edges.shape
    points = np.array([[(0, rows), (0, 90), (319, 90), (cols, rows)]])

    roi_edges = roi_mask(edges, points)

    roi_edges = process_image(roi_edges)

    b,slope = getRLaneLine(roi_edges)
    x0 = (int)((179 - b) / slope)
    print('slope:',slope)
    print('b:',b)
    print('x0:',x0)

img_path = 'capture/lane.jpg'    
get_x0(img_path)    
